#include <ros/ros.h>
#include <geometry_msgs/Point.h>
#include <vector>
#include <fstream>
#include <string>

// 结构体定义（与你主程序保持一致）
struct gnc_api_waypoint {
    float x;
    float y;
    float z;
    float psi;
};

// 保存目标点到的容器
std::vector<gnc_api_waypoint> waypoints2;

// 接收到的目标点回调
void targetCallback(const geometry_msgs::Point::ConstPtr& msg)
{
    gnc_api_waypoint wp;
    wp.x = msg->x;
    wp.y = msg->y;
    wp.z = msg->z;
    wp.psi = 0;  // 默认朝向角为0（如不使用可忽略）

    // 追加到全局变量
    waypoints2.push_back(wp);

    ROS_INFO("Received and saved: x=%.2f, y=%.2f, z=%.2f", wp.x, wp.y, wp.z);
}

// 保存到 CSV 函数
void save_waypoints2_to_csv(const std::string& filename) {
    std::ofstream file(filename);
    if (!file.is_open()) {
        ROS_ERROR("Failed to open file: %s", filename.c_str());
        return;
    }

    file << "x,y,z,psi\n";
    for (const auto& wp : waypoints2) {
        file << wp.x << "," << wp.y << "," << wp.z << "," << wp.psi << "\n";
    }

    file.close();
    ROS_INFO("Saved %lu waypoints to %s", waypoints2.size(), filename.c_str());
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "target_xyz_saver");
    ros::NodeHandle nh;

    // 订阅 /target_xyz
    ros::Subscriber target_sub = nh.subscribe("/target_xyz", 10, targetCallback);
    ros::Time starttime=ros::Time::now();
    ROS_INFO("Listening to /target_xyz... Press Ctrl+C to exit and save.");

    // 主循环
    ros::Rate rate(10);
    while (ros::ok()) {
        ros::spinOnce();
        if ((ros::Time::now()-starttime).toSec()>=20.0){
            save_waypoints2_to_csv("/home/jetson/waypoints2_test2.csv");
            break;
        }
        rate.sleep();


    }

    // 退出时保存数据
    
    return 0;
}